/* Copyright 2017 The Apollo Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=========================================================================*/
#include <string>
#include <vector>

#include "modules/map/hdmap/adapter/odr_parser/lanes_odr_parser.h"
#include "modules/map/hdmap/adapter/odr_parser/objects_odr_parser.h"
#include "modules/map/hdmap/adapter/odr_parser/roads_odr_parser.h"
#include "modules/map/hdmap/adapter/odr_parser/signals_odr_parser.h"
#include "modules/map/hdmap/adapter/xml_parser/util_xml_parser.h"

namespace {
double ToMPS(double speed) { return speed * 1000.0 / 3600.0; }

bool IsRoadBelongToJunction(const std::string& road_id) {
  CHECK(!road_id.empty());
  return road_id != "-1";
}
}  // namespace

namespace apollo {
namespace hdmap {
namespace adapter {

Status RoadsOdrParser::Parse(const tinyxml2::XMLElement& xml_node,
                             std::map<std::string, Road*>& roads) {
//  CHECK_NOTNULL(roads);

  auto road_node = xml_node.FirstChildElement("road");
  while (road_node) {
    // road attributes
    Road* road = new Road();
    // TODO: parse name attribute, need to be wschar

    int checker = UtilXmlParser::QueryStringAttribute(*road_node, "id", &(road->id));
    checker += UtilXmlParser::QueryStringAttribute(*road_node, "junction",
                                                   &road->junction_id);
    //printf("road id: %s\n", road->id.c_str());
    if (checker != tinyxml2::XML_SUCCESS) {
      std::string err_msg = "Error parsing road attributes";
      return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }
    checker = road_node->QueryDoubleAttribute("length", &road->length);
    if (checker != tinyxml2::XML_SUCCESS) {
      std::string err_msg = "Error parse road length attribute";
      return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }

    // type
    RETURN_IF_ERROR(ParseType(*road_node, *road));

    // lanes
    RETURN_IF_ERROR(LanesOdrParser::Parse(road->id, *road_node, &road->laneSections));
    
    // link
    RETURN_IF_ERROR(ParseLink(*road_node, road->link));

    // planView
    RETURN_IF_ERROR(ParsePlanView(*road_node, *road));

    // elevationProfile
    RETURN_IF_ERROR(ParseElevationProfile(*road_node, *road));

    // lateralProfile
    RETURN_IF_ERROR(ParseLateralProfile(*road_node, *road));

    // objects
    RETURN_IF_ERROR(ObjectsOdrParser::ParseObjects(*road_node, *road));

    // signals
    RETURN_IF_ERROR(SignalsOdrParser::ParseSignals(*road_node, *road));

    roads.insert(std::make_pair(road->id, road));
    road_node = road_node->NextSiblingElement("road");
  }

  return Status::OK();
}

Status RoadsOdrParser::ParseType(const tinyxml2::XMLElement& xml_node,
                                  Road &road_internal)
{
  road_internal.type = NULL;

  auto sub_node = xml_node.FirstChildElement("type");
  if (sub_node) {
    RoadType* type = new RoadType();
    int checker = sub_node->QueryDoubleAttribute("s", &type->s);
    if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse road type s attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }

    std::string roadType;
    checker = UtilXmlParser::QueryStringAttribute(*sub_node, "type", &roadType);
    if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse road type type attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }
    std::string upper_str = UtilXmlParser::ToUpper(roadType);
    if (upper_str == "UNKNOWN")
        type->type = ROADTYPE_UNKNOWN;
    else if (upper_str == "RURAL")
        type->type = ROADTYPE_RURAL;
    else if (upper_str == "MOTORWAY")
        type->type = ROADTYPE_MOTORWAY;
    else if (upper_str == "TOWN")
        type->type = ROADTYPE_TOWN;
    else if (upper_str == "LOWSPEED")
        type->type = ROADTYPE_LOWSPEED;
    else if (upper_str == "PEDESTRIAN")
        type->type = ROADTYPE_PEDESTRIAN;
    else if (upper_str == "BICYCLE")
        type->type = ROADTYPE_BICYCLE;

    type->maxSpeed = 0.0;
    auto speed_node = sub_node->FirstChildElement("speed");
    if (speed_node) {
      std::string speedStr;
      double speed;
      int checker = UtilXmlParser::QueryStringAttribute(*speed_node, "max", &speedStr);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse road type - speed max attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }
      if (speedStr == "no limit" || speedStr == "undefined") {
        speed = 0.0;
      } else {
        speed = atof(speedStr.c_str());
        if (speed == 0.0) {
          printf("Warning: wrong max speed string for the road");
        }
      }
      std::string unit;
      checker = UtilXmlParser::QueryStringAttribute(*speed_node, "unit", &unit);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse road type - speed unit attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      std::string upper_str = UtilXmlParser::ToUpper(unit);
      if (unit == "KM/H")
          type->maxSpeed = ToMPS(speed);
      else if (unit == "M/S")
          type->maxSpeed = speed;
      else if (unit == "MPH") {
        std::string err_msg = "Error unsuported road type - speed unit attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }
    }

    road_internal.type = type;
  }

  return Status::OK();
}

// TODO
Status RoadsOdrParser::ParseLink(const tinyxml2::XMLElement& xml_node,
                                  Link &link) {
  link.predecessor = NULL;
  link.successor = NULL;
  auto sub_node = xml_node.FirstChildElement("link");
  if (sub_node) {
    std::string sub_name = "predecessor";
    link.predecessor = new Predecessor();
    RETURN_IF_ERROR(ParsePredecessor(*sub_node, sub_name, link.predecessor));

    sub_name = "successor";
    link.successor = new Predecessor();
    RETURN_IF_ERROR(ParsePredecessor(*sub_node, sub_name, link.successor));
  }

  return Status::OK();
}

Status RoadsOdrParser::ParsePredecessor(const tinyxml2::XMLElement& xml_node, 
                                           const std::string& child_name,
                                           Predecessor* &pre) {
    int checker = tinyxml2::XML_SUCCESS;
    auto node = xml_node.FirstChildElement(child_name.c_str());
    if (node) {
      //pre = new Predecessor();

      std::string elementType;
      checker = UtilXmlParser::QueryStringAttribute(*node, "elementType", &elementType);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse link predecessor elementType attribute";
        delete pre;
        pre = NULL;
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }
      if (elementType == "junction") {
        pre->elementType = JUNCTION;
      } else if (elementType == "road") {
        pre->elementType = ROAD;
      } else {
        delete pre;
        pre = NULL;
        std::string err_msg = "Invalid link predecessor elementType attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      checker = UtilXmlParser::QueryStringAttribute(*node, "elementId", &pre->elementId);
      if (checker != tinyxml2::XML_SUCCESS) {
        delete pre;
        pre = NULL;
        std::string err_msg = "Error parse link predecessor elementId attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      pre->contactPoint = START;
      std::string contactPoint;
      checker = UtilXmlParser::QueryStringAttribute(*node, "contactPoint", &contactPoint);
      if (contactPoint == "end") {
        pre->contactPoint = END;
      }
	  else if (contactPoint == "start") {
		pre->contactPoint = START;
	  }
	  else
	  {
		std::string err_msg = "Error parse link contactPoint attribute";
		return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
	  }
    }
    else {
        delete pre;
        pre = NULL;
    }

  return Status::OK();
}

// TODO
Status RoadsOdrParser::ParsePlanView(const tinyxml2::XMLElement& xml_node,
                                  Road &road) {
  auto sub_node = xml_node.FirstChildElement("planView");
  if (sub_node) {
    auto geo_node = sub_node->FirstChildElement("geometry");
    while (geo_node) {
      Geometry* geo = new Geometry;
      int checker = geo_node->QueryDoubleAttribute("s", &geo->s);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse geometry s attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      checker = geo_node->QueryDoubleAttribute("x", &geo->x);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse geometry x attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      checker = geo_node->QueryDoubleAttribute("y", &geo->y);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse geometry y attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      checker = geo_node->QueryDoubleAttribute("hdg", &geo->hdg);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse geometry hdg  attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      checker = geo_node->QueryDoubleAttribute("length", &geo->length);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse geometry length attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      RETURN_IF_ERROR(ParseGeometryChild(*geo_node, geo));

      road.planView.geometrys.push_back(geo);
      geo_node = geo_node->NextSiblingElement("geometry");
    }
  }

  return Status::OK();
}

Status RoadsOdrParser::ParseGeometryChild(const tinyxml2::XMLElement& geo_node,
                                                Geometry* geo)
{
  int checker;

  auto child_node = geo_node.FirstChildElement("line");
  if(child_node) {
    geo->type = GEO_LINE;
  } 
  if (!child_node) {
    child_node = geo_node.FirstChildElement("paramPoly3");
    if (child_node) {
      ParaPoly3& poly3 = geo->poly3;
      geo->type = GEO_PARAPOLY3;

      checker = child_node->QueryDoubleAttribute("aU", &poly3.aU);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse geometry ParaPoly aU attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }
      
      checker = child_node->QueryDoubleAttribute("bU", &poly3.bU);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse geometry ParaPoly bU attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }
      
      checker = child_node->QueryDoubleAttribute("cU", &poly3.cU);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse geometry ParaPoly cU attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      checker = child_node->QueryDoubleAttribute("dU", &poly3.dU);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse geometry ParaPoly dU attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      checker = child_node->QueryDoubleAttribute("aV", &poly3.aV);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse geometry ParaPoly aV attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      checker = child_node->QueryDoubleAttribute("bV", &poly3.bV);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse geometry ParaPoly bV attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      checker = child_node->QueryDoubleAttribute("cV", &poly3.cV);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse geometry ParaPoly cV attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }
       
      checker = child_node->QueryDoubleAttribute("dV", &poly3.dV);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse geometry ParaPoly dV attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }
    }  

    if (!child_node) {
      // arc
      child_node = geo_node.FirstChildElement("arc");
      if (child_node) {
        geo->type = GEO_ARC;

        checker = child_node->QueryDoubleAttribute("curvature", &geo->arc_curvature);
        if (checker != tinyxml2::XML_SUCCESS) {
          std::string err_msg = "Error parse geometry arc curvature attribute";
          return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
        }
      }
    }  

    if (!child_node) {
      // spiral
      child_node = geo_node.FirstChildElement("spiral");
      if (child_node) {
        geo->type = GEO_SPIRAL;

        checker = child_node->QueryDoubleAttribute("curvStart", &geo->spiral_curve_start);
        if (checker != tinyxml2::XML_SUCCESS) {
          std::string err_msg = "Error parse geometry spiral curve start attribute";
          return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
        }

        checker = child_node->QueryDoubleAttribute("curvEnd", &geo->spiral_curve_end);
        if (checker != tinyxml2::XML_SUCCESS) {
          std::string err_msg = "Error parse geometry spiral curve end attribute";
          return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
        }
      }
    }
  }

  if (!child_node) {
        std::string err_msg = "Error parse geometry child element";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }

  return Status::OK();
}

Status RoadsOdrParser::ParseElevation(const tinyxml2::XMLElement& elevation_node,
                                  Elevation& elevation)
{
    int checker = elevation_node.QueryDoubleAttribute("s", &elevation.s);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse elevation s attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      checker = elevation_node.QueryDoubleAttribute("a", &elevation.a);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse elevation a attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }
      checker = elevation_node.QueryDoubleAttribute("b", &elevation.b);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse elevation b attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }
      checker = elevation_node.QueryDoubleAttribute("c", &elevation.c);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse elevation c attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      checker = elevation_node.QueryDoubleAttribute("d", &elevation.d);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse elevation d attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      return Status::OK();
}

Status RoadsOdrParser::ParseElevationProfile(const tinyxml2::XMLElement& xml_node,
                                  Road &road_internal)
{  
  auto elevation_profile_node = xml_node.FirstChildElement("elevationProfile");
  if (elevation_profile_node) {    
    auto elevation_node = elevation_profile_node->FirstChildElement("elevation");
    while (elevation_node) {
      Elevation elevation;
      RETURN_IF_ERROR(ParseElevation(*elevation_node, elevation));

      road_internal.elevationProfile.push_back(elevation);
      elevation_node = elevation_profile_node->NextSiblingElement("elevation");
    }
  }

  return Status::OK();
}

Status RoadsOdrParser::ParseLateralProfile(const tinyxml2::XMLElement& xml_node,
                                  Road &road_internal)
{
  road_internal.lateralProfile = NULL;

  auto lateral_profile_node = xml_node.FirstChildElement("lateralProfile");
  if (lateral_profile_node) {
    LateralProfile* profile = new LateralProfile();

    auto superelevation_node = lateral_profile_node->FirstChildElement("superelevation");
    while (superelevation_node) {
      Elevation elevation;
      RETURN_IF_ERROR(ParseElevation(*superelevation_node, elevation));

      profile->superelevations.push_back(elevation);
      superelevation_node = lateral_profile_node->NextSiblingElement("superelevation");
    }

    auto crossfall_node = lateral_profile_node->FirstChildElement("crossfall");
    while (crossfall_node) {
      Elevation elevation;
      RETURN_IF_ERROR(ParseElevation(*crossfall_node, elevation));

      profile->crossfalls.push_back(elevation);
      crossfall_node = lateral_profile_node->NextSiblingElement("elevation");
    }

    road_internal.lateralProfile = profile;
  }

  return Status::OK();

}

}  // namespace adapter
}  // namespace hdmap
}  // namespace apollo
